//
//#include <stdio.h>
//#include <stdlib.h>
//#include <math.h>
//#include "WARDEN.h"
//
//Coordinate Origin;
//Coordinate bounds [BOUND_POINTS];
//
//
//Warden::Warden(){
//
//}
//
//Warden::Warden(Coordinate orgn){
//	Origin = orgn;
//}
//
//void Warden::createBoundsArray ()
//    {
//    	Coordinate a (42.39388815, -72.52890826);
//    	Coordinate b (42.39365914, -72.52965247);
//    	Coordinate c (42.39332142, -72.52946627);
//    	Coordinate d (42.39358300, -72.52873163);
//    	bounds[0] = a;
//    	bounds[1] = b;
//    	bounds[2] = c;
//    	bounds[3] = d;
//    }
//
//
//bool dubEq(double a, double b){
//
//	double diff=0;
//	diff = a-b;
//	if( (diff<(.0001)) && (diff>(-.0001)) ) return true;
//	else return false;
//
//}
//
//bool Warden::onSameSide (Coordinate test)
//{
//	double lat = test.getLat();
//	double lon = test.getLon();
//	Coordinate bounds1, bounds2;
//	double a, b, c, d, x1, y1;
//	bool given = false, testry=false, inbounds=true;
//
//	for(int i=0; i<BOUND_POINTS; i++)
//	{
//		bounds1 = bounds[i];
//		if(i==BOUND_POINTS-1)
//		{
//			bounds2 = bounds[0];
//		}
//		else
//		{
//			bounds2 = bounds[i+1];
//		}
//			
//		a = bounds1.getLon();
//		b = bounds1.getLat();
//		c = bounds2.getLon();
//		d = bounds2.getLat();
//		x1 = Origin.getLon();
//		y1 = Origin.getLat();
//			
//		given = y1-b<(d-b)/(c-a)*(x1-a);
//		testry = lat-b<(d-b)/(c-a)*(lon-a);
//			
//		if (given != testry)
//		{
//			inbounds = false;
//		}
//			
//	}
//	return inbounds;
//}
//
//
//bool Warden::inBounds (Coordinate test)
//    {
//    	Coordinate bounds1, bounds2;
//    	int boundsCrossed = 0;
//    	int i;
//    	for(i=0; i<BOUND_POINTS; i++)
//    	{
//    		bounds1 = bounds[i];
//    		if(i==BOUND_POINTS-1)
//    		{
//    			bounds2 = bounds[0];
//    		}
//    		else
//    		{
//    			bounds2 = bounds[i+1];
//    		}
//
//    		/*
//    		* This "if" is to test whether the two lines are parallel, because
//    		* if they are, their intersection will not be valid
//			*/
//    		if(true)   // ((Origin.getLat()-test.getLat()) / (Origin.getLon()-test.getLon())) != (y3-y4)/(x3-x4)) )
//    		{
//    			Coordinate intersect = getIntersection(test, bounds1, bounds2);
//    			if(intersect.getLon()==bounds1.getLon() && intersect.getLat()==bounds1.getLat())
//				//if(dubEq(intersect.getLon(), bounds1.getLon()) && dubEq(intersect.getLat(), bounds1.getLat()))
//				{
//					/*
//					 * This if is put in place so that in the case that the line segemnt
//					 * from origin to test point runs through a boundary point, that
//					 * intersection will be counted only once and not twice. When this point
//					 * assumes the role of bounds2 (every point gets to be both bounds1
//					 * and bounds2 at some point) then this intersection will be counted.
//					 * There is also the chance that the intersection of lines will occur on
//					 * a boundary point with the test point inside the bounds. While this will
//					 * return out of bounds, we will take multiple readings before deciding
//					 * that the collar is truly out of bounds, and these multiple readings
//					 * will each be slightly different.
//					 */
//				}
//				else if(intersectionValid(intersect, test, bounds1, bounds2))
//				{
//					boundsCrossed++;
//				}
//    		}
//    	}
//    	///printf("Bounds Crossed: %d\n", boundsCrossed);
//    	if(boundsCrossed%2 == 0)
//		{
//			digitalWrite(4, HIGH);
//			return true;
//		}
//
//		return false;
//    }
//
//
//    /*
//     * This method takes in 3 points (and can reference the origin point) and returns
//     * the intersection of the line formed by the origin and test with the line formed
//     * by the two bounds points
//     */
//Coordinate Warden::getIntersection (Coordinate test, Coordinate b1, Coordinate b2)
//	{
//		double x1,x2,x3,x4,y1,y2,y3,y4,x,y;
//
//		/*
//		 * There are five possible cases:
//		 * 1) The boundary line is vertical, the test-origin line is horizontal
//		 * 2) The boundary line is vertical, the test-origin line isn't horizontal
//		 * 3) The test-origin line is vertical, the boundary line is horizontal
//		 * 4) The test-origin line is vertical, the boundary line isn't horizontal
//		 * 5) Neither line is vertical
//		 *
//		 * Here we deal with each of these cases. We don't care if the lines are parallel,
//		 * because they will never intersect.
//		 */
//		if(b1.getLon()==b2.getLon())
//		{
//			/*
//			 * Case 1: boundary vertical, test-origin horizontal
//			 * Solution: we know the intersection easily
//			 */
//			if(Origin.getLat()==test.getLat())
//			{
//				x = b1.getLon();
//				y = Origin.getLat();
//			}
//
//			/*
//			 * Case 2: boundary vertical, test-origin not horizontal
//			 * Solution: flip the axes so there are no vertical lines
//			 */
//			else
//			{
//				y1 = Origin.getLon();
//				y2 = test.getLon();
//				y3 = b1.getLon();
//				y4 = b2.getLon();
//				x1 = Origin.getLat();
//				x2 = test.getLat();
//				x3 = b1.getLat();
//				x4 = b2.getLat();
//
//				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
//				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
//			}
//		}
//		else if(Origin.getLon()==test.getLon())
//		{
//			/*
//			 * Case 3: test-origin vertical, boundary horizontal
//			 * Solution: we know the intersection easily
//			 */
//			if(b1.getLat()==b2.getLat())
//			{
//				x = Origin.getLon();
//				y = b1.getLat();
//			}
//
//			/*
//			 * Case 4: test-origin vertical, boundary not horizontal
//			 * Solution: flip axes so there are no vertical lines
//			 */
//			else
//			{
//				y1 = Origin.getLon();
//				y2 = test.getLon();
//				y3 = b1.getLon();
//				y4 = b2.getLon();
//				x1 = Origin.getLat();
//				x2 = test.getLat();
//				x3 = b1.getLat();
//				x4 = b2.getLat();
//
//				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
//				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
//			}
//		}
//
//		/*
//		 * Case 5: neither line is vertical
//		 * Solution: use the intersection of two lines formula
//		 */
//		else
//		{
//			x1 = Origin.getLon();
//			x2 = test.getLon();
//			x3 = b1.getLon();
//			x4 = b2.getLon();
//			y1 = Origin.getLat();
//			y2 = test.getLat();
//			y3 = b1.getLat();
//			y4 = b2.getLat();
//
//			x = ( ((x1*(y2-y1))/(x2-x1)) - ( ((x3*(y4-y3))/(x4-x3))+y3-y1 ))   /   ( ((y2-y1)/(x2-x1))-((y4-y3)/(x4-x3)) );
//			y = (y2-y1)/(x2-x1)*(x-x1)+y1;
//		}
//
//		return Coordinate (x, y);
//	}
//
//	/*
//	 * This method takes a given intersection of two lines and checks whether the intersection
//	 * occurs within the two lines segments formed by the four points
//	 */
//bool Warden::intersectionValid (Coordinate intersection, Coordinate test, Coordinate b1, Coordinate b2)
//	{
//		double x1 = Origin.getLon();
//		double x2 = test.getLon();
//		double x3 = b1.getLon();
//		double x4 = b2.getLon();
//		double y1 = Origin.getLat();
//		double y2 = test.getLat();
//		double y3 = b1.getLat();
//		double y4 = b2.getLat();
//		double interX = intersection.getLon();
//		double interY = intersection.getLat();
//
//		/*
//		 * A bit of craziness: just found a bug with this. If the line drawn from
//		 * the origin to the test point intersects the bounds AT A BOUNDARY POINT,
//		 * then the program will think it has crossed two lines and call it inbounds.
//		 * I fixed this with an "if" statement in the "inbounds" method. It executes
//		 * no code and has a comment explaining its purpose.
//		 */
//
//		if(x1>=x2)
//		{
//			if(!(x1>=interX && interX>=x2))
//			{
//				return false;
//			}
//		}
//		else
//		{
//			if(!(x2>=interX && interX>=x1))
//			{
//				return false;
//			}
//		}
//		if(x3>=x4)
//		{
//			if(!(x3>=interX && interX>=x4))
//			{
//				return false;
//			}
//		}
//		else
//		{
//			if(!(x4>=interX && interX>=x3))
//			{
//				return false;
//			}
//		}
//
//
//		if(y1>=y2)
//		{
//			if(!(y1>=interY && interY>=y2))
//			{
//				return false;
//			}
//		}
//		else
//		{
//			if(!(y2>=interY && interY>=y1))
//			{
//				return false;
//			}
//		}
//		if(y3>=y4)
//		{
//			if(!(y3>=interY && interY>=y4))
//			{
//				return false;
//			}
//		}
//		else
//		{
//			if(!(y4>=interY && interY>=y3))
//			{
//				return false;
//			}
//		}
//		return true;
//	}
//
//double Warden::shortestDistance (Coordinate test)
//	{
//		int i;
//		double Ax, Bx, Cx, Ay, By, Cy, a, b, c, r;
//		Coordinate bounds1, bounds2;
//		double tempDistance;
//		double distance = 999999999;
//
//		/*
//		 * Iterate through sets of adjacent boundary points
//		 */
//		for(i=0; i<BOUND_POINTS; i++)
//		{
//			bounds1 = bounds[i];
//			if(i==BOUND_POINTS-1)
//			{
//				bounds2 = bounds[0];
//			}
//			else
//			{
//				bounds2 = bounds[i+1];
//			}
//			Cx = test.getLon();
//			Ax = bounds1.getLon();
//			Bx = bounds2.getLon();
//			Cy = test.getLat();
//			Ay = bounds1.getLat();
//			By = bounds2.getLat();
//
//			/*
//			 * r represents where the perpendicular intersection of the test point intersects the line segment formed by
//			 * the two boundary points. If r<0 the intersection occurs outside of bounds1. If r>1 the intersection
//			 * occurs outside of bounds2. If 0<r<1 the intersection occurs on the line segment.
//			 */
//			r = ((Cx-Ax)*(Bx-Ax)+(Cy-Ay)*(By-Ay))/pow(sqrt(pow((Bx-Ax),2)+pow((By-Ay),2)),2);
//
//			if (0<r && r<1)
//			{
//				/*
//				 * This tests whether the given boundary line segment is a vertical line. If it is, we swap all x and y
//				 * coordinates for both boundary points and the test point. This allows us to sidestep the vertical line
//				 * problem and calculate the accurate distance.
//				 */
//				if(Ax==Bx)
//				{
//					a = (Bx-Ax)/(By-Ay);
//					b = -1;
//					c = Ax - ((Bx-Ax)/(By-Ay))*Ay;
//					Cx = test.getLat();
//					Cy = test.getLon();
//				}
//				else
//				{
//					a = (By-Ay)/(Bx-Ax);
//					b = -1;
//					c = Ay - ((By-Ay)/(Bx-Ax))*Ax;
//				}
//
//				/*
//				 * The absolute value function was truncating our doubles to integers, so we implemented our own here.
//				 * We use the point-line distance formula.
//				 */
//				if(a*Cy+b*Cx+c >= 0)
//				{
//					tempDistance = (a*Cx+b*Cy+c)/sqrt(a*a+b*b);
//				}
//				else
//				{
//					tempDistance = -1*(a*Cx+b*Cy+c)/sqrt(a*a+b*b);
//				}
//
//				/*
//				 * We only want to consider positive distances- otherwise the smallest distance will be the greatest
//				 * negative number
//				 */
//				if(tempDistance < 0)
//				{
//					tempDistance *= -1;
//				}
//
//				if (distance>tempDistance)
//				{
//					distance=tempDistance;
//				}
//			}
//			/*
//			 * If the perpendicular intersection occurs outside of the line segment, we calculate the distance to
//			 * each of the two boundary points.
//			 */
//			else
//			{
//				tempDistance = sqrt(pow((Cx-Ax),2)+pow((Cy-Ay),2));
//				if (distance>tempDistance)
//				{
//					distance=tempDistance;
//				}
//
//				tempDistance = sqrt(pow((Cx-Bx),2)+pow((Cy-By),2));
//				if (distance>tempDistance)
//				{
//					distance=tempDistance;
//				}
//			}
//
//		}
//		return distance;
//}